/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "motor.h"
#include "rtthread.h"
typedef struct f16_pid
{
    float target;
    float speed;
    float kp;
    float ki;
    float kd;
    float err;
    float err_last;
    float err_integral;
    float output;
} int16_pid_t;

int16_pid_t m1_speed_pid = {0};
void int16_pid_init(int16_pid_t *pid, float kp, float ki, float kd)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->err_integral = 0;
    pid->err_last = 0;
}
void m1_speed_pid_init(float kp, float ki, float kd)
{
    // int p = kp * 1000;
    // int i = ki * 1000;
    // int d = kd * 1000;
    // rt_kprintf("kp=%d,ki=%d,kd=%d\n", p, i, d);
    int16_pid_init(&m1_speed_pid, kp, ki, kd);
}
float int16_pid_calc(int16_pid_t *pid, float input, float dtSeconds)
{
    pid->err = pid->target - input;
    float kiErr = pid->ki * pid->err * dtSeconds;
    if (pid->err_integral > 1000)
    {
        if (pid->err < 0)
        {
            pid->err_integral += kiErr;
        }
    }
    else if (pid->err_integral < -1000)
    {
        if (pid->err > 0)
        {
            pid->err_integral += kiErr;
        }
    }
    else
    {
        pid->err_integral += kiErr;
    }

    pid->output = pid->kp * pid->err + pid->err_integral + pid->kd * (pid->err - pid->err_last) / dtSeconds;

    if (pid->output > 1000)
    {
        pid->output = 1000;
        pid->err_integral = 0;
    }
    else if (pid->output < -1000)
    {
        pid->output = -1000;
        pid->err_integral = 0;
    }
    pid->err_last = pid->err;

    // rt_kprintf("pid: ");
    // rt_kprintf("%d, ",(int)pid->target);
    // rt_kprintf("%d, ",(int)pid->err);
    // rt_kprintf("%d, ", (int)pid->err_integral);
    // rt_kprintf("%d, ", (int)pid->output);
    // rt_kprintf("%d", (int)input);
    // rt_kprintf("\n");
    return pid->output;
}
float motor1_pid_control(int32_t setCycle, float setAngle, float setSpeed, float speedInput, float dtMs)
{
    // int16_pid_init(&m1_speed_pid, 0.3, 0.01, 0);
    m1_speed_pid.target = setSpeed;
    return int16_pid_calc(&m1_speed_pid, speedInput, dtMs);
}
